#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <string.h>
#include <sys/mman.h>
#include <assert.h>

#define CLEAR(x) memset(&(x), 0, sizeof(x))

int fd;

__u32 formatIn;
/* Our current format uses 1 planes per buffer */
#define FMT_NUM_PLANES 1
#define NBUF 20

struct v4l2_plane planes[FMT_NUM_PLANES];

int init_camera(const char *dev) {
	fd = open(dev, O_RDWR);
	if (fd < 0) {
		printf("open \"%s\" error\n", dev);
		return -1;
	}
	printf("VIDIOC_ENUM_FMT start\n");

	struct v4l2_fmtdesc fmtdesc;
	fmtdesc.index = 0;
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) != -1) {
		printf("\t%d.%s\n", fmtdesc.index + 1, fmtdesc.description);
		fmtdesc.index++;
	}
	printf("VIDIOC_ENUM_FMT end\n");
	return 0;
}

struct v4l2_queryctrl queryctrl;
struct v4l2_querymenu querymenu;

//https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/control.html#control-id
static void enumerate_menu(void) {
	for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1;
			queryctrl.id++) {
		if (0 == ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl)) {
			if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
				continue;
			printf("Control = %s \n", queryctrl.name);
			if (queryctrl.type == V4L2_CTRL_TYPE_MENU) {
				printf("V4L2_CTRL_TYPE_MENU\n");
			}

		} else {

		}

	}

//    printf("  Menu items:\n");
//    memset(&querymenu, 0, sizeof(querymenu));
//    querymenu.id = queryctrl.id;
//
//    for (querymenu.index = queryctrl.minimum;
//         querymenu.index <= queryctrl.maximum;
//         querymenu.index++) {
//        if (0 == ioctl(fd, VIDIOC_QUERYMENU, &querymenu)) {
//            printf("  %s\\n", querymenu.name);
//        }
//    }
}

void fourcc_toString(__u32 a, char *name) {
	name[0] = a & 0xff;
	name[1] = (a >> 8) & 0xff;
	name[2] = (a >> 16) & 0xff;
	name[3] = (a >> 24) & 0xff;
	name[4] = '\0';
}

/**
 * https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/vidioc-g-fmt.html#vidioc-g-fmt
 */
int get_format(void) {
	int ret;
	char name[5];
	struct v4l2_format fmt;
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	ret = ioctl(fd, VIDIOC_G_FMT, &fmt);
	if (0 == ret) {
		printf("get format ok \n");
		printf("get_format width %d \n", fmt.fmt.pix_mp.width);
		printf("get_format height %d \n", fmt.fmt.pix_mp.height);
		printf("get_format num_planes %d \n", fmt.fmt.pix_mp.num_planes);
		fourcc_toString(fmt.fmt.pix_mp.pixelformat, name);
		formatIn = fmt.fmt.pix_mp.pixelformat;
		if (formatIn == V4L2_PIX_FMT_YUYV) {
			printf("formatIn is V4L2_PIX_FMT_YUYV\n");
		}
	} else {
		perror("VIDIOC_G_FMT");
	}
	return ret;
}

int set_format(void) {
	int ret;
	struct v4l2_format fmt;
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	fmt.fmt.pix.width = 1296;
	fmt.fmt.pix.height = 972;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
	ret = ioctl(fd, VIDIOC_S_FMT, &fmt);
	if (ret == 0) {
		printf("set format ok \n");
	} else {
		perror("VIDIOC_S_FMT");
	}
	return ret;
}

/**
 * https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/vidioc-enuminput.html#vidioc-enuminput
 */
void enum_input(void) {
	struct v4l2_input input;
	memset(&input, 0, sizeof(input));
	input.index = -2;
	if (-1 == ioctl(fd, VIDIOC_ENUMINPUT, &input)) {
		perror("VIDIOC_ENUMINPUT");
		exit(EXIT_FAILURE);
	}
	printf("Current input.name: %s \n", input.name);
	printf("Current input.status: %d \n", input.status);
	printf("Current input.capabilities: %d \n", input.capabilities);
	if (input.type == V4L2_INPUT_TYPE_CAMERA) {
		printf("input.type = V4L2_INPUT_TYPE_CAMERA   \n");
	}
}

void get_input(void) {
	int index = -1;
	if (-1 == ioctl(fd, VIDIOC_G_INPUT, &index)) {
		perror("VIDIOC_G_INPUT");
//		exit(EXIT_FAILURE);
	}
	printf("input index: %d\n", index);
}

int query_capabilities(void) {
	struct v4l2_capability cap;
	int ret = ioctl(fd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0) {
		printf("VIDIOC_QUERYCAP error\n");
		return -1;
	}

	printf("driver : %s\n", cap.driver);
	printf("device : %s\n", cap.card);
	printf("bus : %s\n", cap.bus_info);
	printf("capabilities : %x\n", cap.capabilities);

	if (cap.capabilities & V4L2_CAP_READWRITE == V4L2_CAP_READWRITE) {
		printf("V4L2_CAP_READWRITE \n");
	} else {
		printf("no support for io read  \n");
	}

	printf("is video capture: %s\n",
			cap.capabilities & V4L2_CAP_VIDEO_CAPTURE ? "True" : "False");
	printf("support io read: %s\n",
			cap.capabilities & V4L2_CAP_READWRITE ? "True" : "False");
	printf("support IO stream: %s\n",
			cap.capabilities & V4L2_CAP_STREAMING ? "True" : "False");
	return 0;
}

struct {
	void *start[FMT_NUM_PLANES];
	size_t length[FMT_NUM_PLANES];
} *buffers;

unsigned long long i, j;

int request_bufs(void) {
	struct v4l2_requestbuffers reqbuf;
	reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	reqbuf.memory = V4L2_MEMORY_MMAP;
	reqbuf.count = NBUF;
	// 请求在内核空间分配视频缓冲区
	int ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
	if (ret < 0) {
		perror("VIDIOC_REQBUFS");
		return -1;
	}

	printf("VIDIOC_REQBUFS success, %d\n", reqbuf.count);

	if (reqbuf.count < 5) {
		/* You may need to free the buffers here. */
		printf("Not enough buffer memory\\n");
		exit(EXIT_FAILURE);
	}

	buffers = calloc(reqbuf.count, sizeof(*buffers));
	assert(buffers != NULL);

	for (i = 0; i < reqbuf.count; i++) {
		struct v4l2_buffer buffer;

		memset(&buffer, 0, sizeof(buffer));
		buffer.type = reqbuf.type;
		buffer.memory = V4L2_MEMORY_MMAP;
		buffer.index = i;
		/* length in struct v4l2_buffer in multi-planar API stores the size
		 * of planes array. */
		buffer.length = FMT_NUM_PLANES;
		buffer.m.planes = planes;

//		 查询分配好的buffer信息
		if (ioctl(fd, VIDIOC_QUERYBUF, &buffer) < 0) {
			perror("VIDIOC_QUERYBUF");
			exit(EXIT_FAILURE);
		}

		/* Every plane has to be mapped separately */
		for (j = 0; j < FMT_NUM_PLANES; j++) {
			buffers[i].length[j] = buffer.m.planes[j].length; /* remember for munmap() */

			buffers[i].start[j] = mmap(NULL, buffer.m.planes[j].length,
			PROT_READ | PROT_WRITE, /* recommended */
			MAP_SHARED, /* recommended */
			fd, buffer.m.planes[j].m.mem_offset);

			if (MAP_FAILED == buffers[i].start[j]) {
				perror("mmap");
				exit(EXIT_FAILURE);
			}
		}
		if (ioctl(fd, VIDIOC_QBUF, &buffer) < 0) { // 若映射成功则将内核缓冲区入队
			perror("VIDIOC_QBUF");
			return -1;
		}
		printf("request_bufs success \n");
	}
	printf("request_bufs end success \n");
	return 0;
}
int file;

int display(void *start, size_t length) {
	printf("display success\n");
	if(write(file, start, length)<0)
	{
		perror("write");
	}
	return 0;
}

static int loop() {
	int ret;
	struct v4l2_buffer v4lbuf;
	bzero(&v4lbuf, sizeof(v4lbuf));
	v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	v4lbuf.memory = V4L2_MEMORY_MMAP;
	v4lbuf.reserved = 0;
	v4lbuf.length = FMT_NUM_PLANES;
	v4lbuf.m.planes = planes;
	v4lbuf.flags = 0;

	file = open("../test/output.yuv", O_WRONLY | O_CREAT);
	if (file == -1) {
		perror("open");
		return -1;
	}

	i = 0;
	while (1) {
		// dequeue
		printf("loop %d \n", i);
		// 从队列中取出填满数据的缓存
		//https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/vidioc-qbuf.html
		ret = ioctl(fd, VIDIOC_DQBUF, &v4lbuf);
		if (ret < 0) {
			perror("VIDIOC_DQBUF");
			return ret;
		} else
			printf("lVIDIOC_DQBUF success\n");

		printf("v4lbuf.index = %d \n", v4lbuf.index);

		if (i == 99) {
			for (j = 0; j < FMT_NUM_PLANES; j++) {
				display(buffers[i% NBUF].start[j], buffers[i%NBUF].length[j]);  //显示取出数据的缓存
			}
		}

		i++;
		//enqueue 将缓存放入队列中
		v4lbuf.flags = 0;
		ret = ioctl(fd, VIDIOC_QBUF, &v4lbuf);
		if (ret < 0) {
			perror("VIDIOC_QBUF");
			return ret;
		}
		if (i == 100) {
			close(file);
			return 0;
		}
	}
	return 0;
}

/**
 * @brief stream_on 开启视频流
 * @return 成功返回0，失败返回-1
 */
int stream_on() {
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	if (ioctl(fd, VIDIOC_STREAMON, &type) < 0) {
		perror("VIDIOC_STREAMON");
		return -1;
	}
	printf("VIDIOC_STREAMON success\n");
	return 0;
}

/**
 * @brief stream_off 视频流
 * @return 成功返回0，失败返回-1
 */
int stream_off() {
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	if (ioctl(fd, VIDIOC_STREAMOFF, &type) < 0) {
		perror("VIDIOC_STREAMOFF");
		return -1;
	}
	printf("VIDIOC_STREAMOFF success\n");
	return 0;
}

/**
 *   step 1:  VIDIOC_REQBUFS
 *   step 2:  VIDIOC_REQBUFS
 *
 */
int main(void) {
	int ret = init_camera("/dev/video0");
	if (ret < 0) {
		printf("init_camera error\n");
		return -1;
	}
	if (query_capabilities() < 0)
		return -1;
	if (set_format() < 0)
		return -1;
	if (get_format() < 0)
		return -1;
	if (request_bufs() < 0)
		return -1;
	if (stream_on() < 0)
		return -1;
	if (loop() < 0)
		return -1;
	if (stream_off() < 0)
		return -1;
	return 0;
}
